//=====================================================================================================
// MahonyAHRS.h
//
// Modified version, use double instead of float for ROS compatibility
//
//=====================================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
//
//=====================================================================================================
#ifndef MahonyAHRS_h
#define MahonyAHRS_h

class MahonyAHRS
{
 private:
  double twoKp;			// 2 * proportional gain (Kp)
  double twoKi;			// 2 * integral gain (Ki)

  double integralFBx;
  double integralFBy;
  double integralFBz;	// integral error terms scaled by Ki

  double sample_period;
//----------------------------------------------------------------------------------------------------
// Variable declaration

 public:
  double q0;
  double q1;
  double q2;
  double q3;	// quaternion of sensor frame relative to auxiliary frame

  MahonyAHRS(double sampleT, double kp, double ki);

//---------------------------------------------------------------------------------------------------
// Function declarations

void AHRSupdate(double gx, double gy, double gz, 
		double ax, double ay, double az, 
		double mx, double my, double mz);
void AHRSupdateIMU(double gx, double gy, double gz, 
		   double ax, double ay, double az);
};
#endif
//=====================================================================================================
// End of file
//=====================================================================================================
